#include "../_includes.h"

// //抢中位粽子；吐通道；吸预装，侧面网放预装；怼墙校准；挖角落粽子；推通道粽子
void auto_left_6() {}
// void auto_left_6() {
//   int dist_fwd = 1000;
//   dig.act_for_enc_absolute(enc_dig_prepare_to_cellect_triball, 100, 1600,
//                            Block::no); // 解锁
//   chassis.stable_forward_dist_relative(dist_fwd, 100, 1500, Block::yes);
//   intake.set_voltage(100); // 开吸
//   dig.act_for_enc_absolute(enc_dig_collect_triball + 15, 100, 1000,
//                            Block::yes); // 挖粽子
//   dig.set_voltage(-15);                 // 稳住
//   chassis.stable_forward_dist_relative(-(dist_fwd - 170), 100, 1500,
//                                        Block::yes);
//   chassis.fast_forward_dist_relative(50, 80, 200, Block::yes); //接粽子
//   wait(200);
//   chassis.fast_forward_dist_relative(-50, 80, 400, Block::yes); //后退到原位置
//   dig.act_for_enc_absolute(enc_dig_upright, 100, 1000, Block::no); // 立起来
//   turn_pid_adjust_agl_absolute(120, 100, 800, Block::yes); // 转向通道
//   intake.set_voltage(-100);                               // 吐出
//   wait(400);
//   turn_pid_adjust_agl_absolute(220, 100, 1100, Block::yes); // 转向颜色粽子

//   chassis.reset_enc();
//   intake.set_voltage(100); // 开吸
//   // dig.act_for_enc_absolute(enc_dig_collect_triball + 20, 100, 1000,
//   //                          Block::no); // 挖护住颜色粽子
//   chassis.fast_forward_dist_relative(130, 100, 500, Block::yes); //走近颜色粽子
//   dig.act_for_enc_absolute(enc_dig_upright, 100, 1000, Block::no); //
//   wait(100);
//   turn_pid_adjust_agl_absolute(300, 100, 750,
//                                Block::yes); // 转向接近平行于三角杆
//   chassis.fast_forward_dist_relative(350, 100, 500, Block::yes); // 走向侧网
//   turn_pid_adjust_agl_absolute(345, 100, 800, Block::yes);       // 转向侧网

//   // 撞进颜色粽子
//   intake.set_voltage(-100);
//   wait(200);
//   chassis.reset_enc();
//   chassis.FT_set_voltage(100, 0);
//   timer t;
//   while (t.time() < 800) {
//     if (chassis.get_dist_mm() > 300)
//       break;
//     if (chassis.get_dist_mm() > 200)
//       intake.set_voltage(15);
//     wait(5);
//   }
//   // repeat(1) { // 补撞
//   //   wait(400);
//   //   chassis.fast_forward_dist_relative(-200, 80, 800, Block::yes);
//   //   wait(300);
//   //   chassis.fast_forward_dist_relative(250, 100, 1300, Block::yes);
//   // }
//   ///////////////
//   chassis.fast_forward_dist_relative(-10, 100, 300,
//                                      Block::yes); //后退一点离开侧网
//   gyro_reset();
//   swing_turn_pid_adjust_agl_absolute(1, 90, 100, 950, Block::yes); //漂到墙
//   chassis.FT_set_voltage(-50, 0);
//   wait(450);    //抵墙
//   gyro_reset(); //墙作为新的陀螺仪0点
//   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//   //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//   chassis.fast_forward_dist_relative(80, 100, 500, Block::yes);
//   dig.act_for_enc_absolute(enc_dig_prepare_to_cellect_triball, 100, 1500,
//                            Block::no); //
//   swing_turn_pid_adjust_agl_absolute(-1, 135, 100, 1050,
//                                      Block::yes); //偏到平行于三角
//   chassis.fast_forward_dist_relative(200, 100, 500, Block::yes);
//   chassis.FT_set_voltage(5, 0); //抵住杆子
//   intake.set_voltage(100);
//   dig.act_for_enc_absolute(enc_dig_collect_triball, 100, 1000,
//                            Block::yes); //挖三角粽子
//   dig.set_voltage(-15);                 //稳住
//   chassis.fast_forward_dist_relative(-340, 100, 1200, Block::yes); //后退吸粽子
//   dig.act_for_enc_absolute(enc_dig_prepare_hit_hang_stick, 100, 1000,
//                            Block::no); //挖挖回位

//   toss.act_for_enc_absolute(155, 100, 1000,
//                             Block::no); //弹射辅助防止过线
//   wait(200);
//   swing_turn_pid_adjust_agl_absolute(1, 3, 100, 1100,
//                                      Block::yes); //漂到通道,稍稍斜着
//   wait(50);
//   intake.stop();
//   dig.stop(hold);
//   intake.mtr_set_voltage(-100); //吐粽子
//   chassis.fast_forward_dist_relative(810, 100, 1500,
//                                      Block::yes); //前进，并碰杆
//   cout << "auctual time:" << t_auto.time() << endl;
//   while_wait(t_auto.time() < 15000);
// }